// Copyright 2014-2017 Oxford University Innovation Limited and the authors of InfiniTAM

#pragma once

#include "../../../ORUtils/SE3Pose.h"
#include "../Misc/ITMPointCloud.h"

namespace ITMLib
{
	/** \brief
	    Stores some internal variables about the current tracking
	    state, most importantly the camera pose
	    存储当前帧状态的一些内部变量，最重要的是相机位姿
	*/
	class ITMTrackingState
	{
	public:
		/** @brief
		    Excerpt of the scene used by the tracker to align
		    a new frame.
		    This is usually the main result generated by the
		    raycasting operation in a ITMLib::Engine::ITMSceneReconstructionEngine.
		    下面的这个pointCloud是raycasting操作中核心生成结果，在这里的主要作用就是去对齐新的帧
		*/
		ITMPointCloud *pointCloud;

		/// The pose used to generate the point cloud.上面那个的位姿（其实就是上一帧的位姿）
		ORUtils::SE3Pose *pose_pointCloud;

		/// Frames processed from start of tracking  track的状态参数之一，统计截止目前这个track总共处理了多少帧
		/// Used as weight in the extended tracker  这个参数再endtend tracker是个权重参数
		int framesProcessed;

        // 点云年龄，我理解是用来估计当前帧的这个从场景中抽取出来的点云越久
		int age_pointCloud;

		/// Current pose of the depth camera. 存储当前深度相机（当前帧）的位姿
		ORUtils::SE3Pose *pose_d;

		/// Tracking quality: 1.0: success, 0.0: track质量：0失败，1成功但质量差，2成功且good。
		enum TrackingResult
		{
			TRACKING_GOOD = 2,
			TRACKING_POOR = 1,
			TRACKING_FAILED = 0
		} trackerResult;

		/// Score associated to the tracking result. 最终结果打分（我理解这是对追踪质量一个定量评价）
		float trackerScore;

		bool HasValidPointCloud(void) const
		{
			return age_pointCloud != -1;
		}//只有点云年龄≠-1，就是有可用的点云（从场景中抽取出来的）

		// 当前帧是否距离点云太远，如果太远就返回true，否则返回false
		bool TrackerFarFromPointCloud(void) const
		{
			// if no point cloud exists, yet 如果点云还没诞生，是一种太远。
			if (age_pointCloud < 0) return true;
			// if the point cloud is older than n frames 如果点云太老，也认为太远。
			if (age_pointCloud > 5) return true;

			Vector3f cameraCenter_pc = -1.0f * (pose_pointCloud->GetR().t() * pose_pointCloud->GetT());
			Vector3f cameraCenter_live = -1.0f * (pose_d->GetR().t() * pose_d->GetT());

			Vector3f diff3 = cameraCenter_pc - cameraCenter_live;

			float diff = diff3.x * diff3.x + diff3.y * diff3.y + diff3.z * diff3.z;

			// if the camera center has moved by more than a threshold 相机移动超过一定距离也认为太远（点云即为上一帧，当前帧和上一帧距离超过一定距离，就认为过远）
			if (diff > 0.0005f) return true;

			return false;
		}

		ITMTrackingState(Vector2i imgSize, MemoryDeviceType memoryType)
		: pointCloud(new ITMPointCloud(imgSize, memoryType)),
			pose_pointCloud(new ORUtils::SE3Pose),
			pose_d(new ORUtils::SE3Pose)
		{
			Reset();
		}

		~ITMTrackingState(void)
		{
			delete pointCloud;
			delete pose_d;
			delete pose_pointCloud;
		}

		void Reset()
		{
			this->age_pointCloud = -1;
			this->pose_d->SetFrom(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
			this->pose_pointCloud->SetFrom(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
			this->trackerResult = TRACKING_GOOD;
			this->trackerScore = 0.0f;
		}

		// Suppress the default copy constructor and assignment operator
		ITMTrackingState(const ITMTrackingState&);
		ITMTrackingState& operator=(const ITMTrackingState&);
	};
}
